#
# Small ctypes wrapper around the Luna shared library
#
#
#

from ctypes import *
from ctypes.util import find_library
import sys


class Luna_Version(Structure):
    _fields_ = [
        ("major", c_ubyte),
        ("minor", c_ubyte)
    ]
    

class Luna_Sensors(Structure):
    _fields_ = [
        ("sonar", c_ushort * 3),
        ("bumper", c_ubyte),
        ("battery", c_ubyte),
        ("charger", c_ubyte),
        ("right_distance", c_ushort),
        ("left_distance", c_ushort),
        ("right_speed", c_ushort),
        ("left_speed", c_ushort)
    ]

class Luna_Drive(Structure):
    _fields_ = [
        ("direction", c_ubyte),
        ("right_speed", c_ubyte),
        ("left_speed", c_ubyte)
    ]


class Luna:

    def __init__(self):
        # use OS-specific heuristic to get path to this lib. 
        #path = find_library("Luna")
        #print(path)
                
        # note we build the shared lib as as cdecl on both Linux and Windows
        if sys.platform == 'win32':
            self.lib = CDLL("Luna.dll")
        else:
            self.lib = CDLL("../libLuna.so")

        if self.lib:
            print("Luna_Init returned ", self.lib.Luna_Init())
            
        self.handle = c_void_p()
        
    def Open(self):
        # LUNA_DLL_API int Luna_Open(void** handle);
        if self.lib:
            return self.lib.Luna_Open(byref(self.handle))
    
    def GetVersion(self):
        # LUNA_DLL_API int Luna_GetVersion(void* handle, Luna_Version);
        if self.lib and self.handle:
            version = Luna_Version()
            status = self.lib.Luna_GetVersion(self.handle, byref(version))
            return "%d.%d" % (version.major, version.minor)
        else:
            return "0.0"
            
    def GetSensors(self):
        if self.lib and self.handle:
            sensors = Luna_Sensors()
            status = self.lib.Luna_GetSensors(self.handle, byref(sensors))
            # return as a dict
            return {"sonar":[sensors.sonar[0],sensors.sonar[1],sensors.sonar[2]], "bumper":sensors.bumper,
                "battery":sensors.battery, "charger":sensors.charger,
                "right_distance":sensors.right_distance, "left_distance":sensors.left_distance,
                "right_speed":sensors.right_speed, "left_speed":sensors.left_speed,
                }
            
    def SetDrive(self, direction, left, right):
        if self.lib and self.handle:
            drive = Luna_Drive()
            drive.direction = direction
            drive.left_speed = left
            drive.right_speed = right
            
            status = self.lib.Luna_SetDrive(self.handle, byref(drive))
            return status
            
    def Close(self):
        # LUNA_DLL_API int Luna_Close(void* handle);
        if self.lib and self.handle:
            self.lib.Luna_Close(self.handle)
            
        
def main():
    luna = Luna()
    
    print("Open returned: ", luna.Open())
    
    version = luna.GetVersion()
    print("Luna board version: %s" % (version))
    
    sensors = luna.GetSensors()
    print("Luna sensors:", sensors)
    
    status = luna.SetDrive(0, 10, 10)
    print("SetDrive returned %d" % (status))
    
    luna.Close()
    

if __name__ == "__main__":
    main()

